#!/usr/bin/env python3.11
import socket
import struct
import time
import sys

# --- 参数检查 ---
if len(sys.argv) != 2 or not sys.argv[1].isdigit() or len(sys.argv[1]) != 2:
    print("Usage: ./replay_path.py <two_digit_id>")
    sys.exit(1)

log_filename = f"./launch_folder/path{sys.argv[1]}.txt"

# --- 配置参数 ---
TARGET_IP = '192.168.1.188'  # 小车的IP
TARGET_PORT = 8888          # 小车的端口

# [新增] 本地通知服务器的配置
NOTIFICATION_IP = '127.0.0.1'       # 本地回环地址
NOTIFICATION_PORT = 5001          # 用于发送完成通知的UDP端口
NOTIFICATION_MESSAGE = b'done'    # 通知消息内容


def replay():
    print(f"--- 开始路径重放: {log_filename} ---")
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

    try:
        with open(log_filename, 'r') as f:
            records = []
            for line in f:
                try:
                    t_str, speed_str, angle_str = line.strip().split(',')
                    t = float(t_str)
                    speed = float(speed_str)
                    angle = float(angle_str)
                    records.append((t, speed, angle))
                except (ValueError, IndexError):
                    continue # 跳过格式错误的行
        if not records:
            print(f"错误: 路径文件 '{log_filename}' 为空或无法解析。")
            sock.close()
            return
    except FileNotFoundError:
        print(f"错误: 找不到路径文件 '{log_filename}'。")
        sock.close()
        return

    start_time = time.time()

    for t, speed, angle in records:
        while time.time() - start_time < t:
            # 使用一个非常短的休眠来让出CPU，同时保持高精度
            time.sleep(0.001)

        packet = struct.pack('ff', speed, angle)
        sock.sendto(packet, (TARGET_IP, TARGET_PORT))
        # 在高频循环中打印会影响性能，可以注释掉或降低频率
        # print(f"Replayed at {t:.4f}s: speed={speed:.4f}, angle={angle:.4f}")

    # --- [核心修改] ---
    # 路径重放完成后，发送一个UDP通知到本地服务器
    try:
        sock.sendto(NOTIFICATION_MESSAGE, (NOTIFICATION_IP, NOTIFICATION_PORT))
        print(f"已向 {NOTIFICATION_IP}:{NOTIFICATION_PORT} 发送完成通知。")
    except Exception as e:
        print(f"发送完成通知失败: {e}")
    # -------------------

    sock.close()
    print("--- 路径重放完成。 ---")


if __name__ == '__main__':
    replay()